#include "./handler_task.h"
#include "../driver/tube.h"
#include "../driver/fridge.h"
#include "../driver/ctrl.h"
#include "../driver/robot_arm1.h"
#include "../driver/robot_arm2.h"
#include "../service/robot_arm.h"
#include "../service/ota.h"
#include "../service/delivery.h"
#include "../service/device_info.h"
#include "../driver/motor_drv.h"

namespace task {

using namespace driver;
using namespace utils;
using namespace service;

static constexpr uint32_t DELIVERY_FLOW_TIMEOUT = 1000;

void HandlerTask::run() {
    uint32_t next_msg_timeout = osWaitForever;
    message_t msg;

    for (;;) {
        osStatus_t ret = m_mq.poll(msg, next_msg_timeout);
        if (ret != osOK) {
            delivery::reset();
            next_msg_timeout = osWaitForever;
            continue;
        }

        uint16_t req = msg.req();
        switch (req) {

            case proto::Preprocess: delivery::pre_proce(msg); break;
            
            case proto::Begin: 
                delivery::begin(msg); 
                next_msg_timeout = DELIVERY_FLOW_TIMEOUT;
            break;
            
            case proto::Run: 
                delivery::run(msg); 
                next_msg_timeout = DELIVERY_FLOW_TIMEOUT;
            break;
            
            case proto::Finish:
                delivery::finish(msg);
                next_msg_timeout = osWaitForever;
            break;

            case proto::Reset: delivery::reset(msg); break;

            case proto::CargoDelivery: delivery::cargo_exec(msg); break;

            case proto::OTAStart: ota::on_start(msg); break;
            case proto::OTATranslate: ota::on_translate(msg); break;
            case proto::OTAComplete: ota::on_complete(msg); break;

            case proto::TestFridge: on_test_fridge(msg); break;
            case proto::SetFridgeArg: on_set_fridge(msg); break;

            case proto::TestTube: on_test_tube(msg); break;
            case proto::SetTubeArg: on_set_tube(msg); break;

            case proto::TestCompr: on_test_compr(msg); break;

            case proto::TestArm: on_test_arm(msg); break;
            case proto::ArmCtrl: on_arm_ctrl(msg); break;
            
            case proto::AutoSetCargo: on_auto_set_cargo(msg); break;
            case proto::SetCargo: on_set_cargo(msg); break;
            case proto::QueryCargo: on_get_cargo(msg); break;
            case proto::QueryAllCargo: on_get_all_cargo(msg); break;

            case proto::ArmAdj: on_arm_adj(msg); break;

            case proto::DelCargo: on_del_cargo(msg); break;
            case proto::TestCargo: on_test_cargo(msg); break;
            case proto::AutoSetPick: on_auto_set_pick(msg); break;
            case proto::SetPick: on_set_pick(msg); break;
            case proto::GetParam: on_get_param(msg); break;
            case proto::ELock: on_open_lock(msg); break;

            case proto::ReadMotorDriver: on_read_motor_driver(msg); break;
            case proto::WriteMotorDriver: on_write_motor_driver(msg); break;
        
            case proto::GetDrvStatus: on_get_drv_status(msg); break;
            case proto::ClearDrvFault: on_clear_drv_status(msg); break;
        }

    }
}

void HandlerTask::on_test_tube(const message_t &msg) {
    uint8_t type;
    uint16_t speed, steps;
    error_t err = msg.parse(type, speed, steps);
    if (err) {
        goto finish;
    }
    switch (type) {
        case 0: err = tube->up(speed, 0); break;
        case 1: err = tube->up(speed, steps); break;
        case 2: err = tube->down(speed, steps); break;
        default: err = ec::ArgInvalid; break;
    }

finish:
    msg.write(proto::TestTube, err);
}

void HandlerTask::on_set_tube(const message_t &msg) {
    uint16_t speed;
    error_t err = msg.parse(speed);
    if (err) {
        goto finish;
    }
    tube->set_target_speed(speed);
finish:
    msg.write(proto::SetTubeArg, err);
}

void HandlerTask::on_test_fridge(const message_t &msg) {
    uint8_t type;
    uint16_t speed;
    error_t err = msg.parse(type, speed);
    if (err) {
        goto finish;
    }
    switch (type) {
        case 0: err = fridge->open(speed); break;
        case 1: err = fridge->close(speed); break;
        default: err = ec::ArgInvalid; break;
    }
finish:
    msg.write(proto::TestFridge, err);
}

void HandlerTask::on_set_fridge(const message_t &msg) {
    uint16_t speed;
    error_t err = msg.parse(speed);
    if (err) {
        goto finish;
    }
    fridge->set_target_speed(speed);
finish:
    msg.write(proto::SetFridgeArg, err);
}

void HandlerTask::on_test_compr(const message_t &msg) {
    uint8_t type;
    error_t err = msg.parse(type);
    if (err) {
        goto finish;
    }
    switch (type) {
        case 0: ctrl::compr_on(); break;
        case 1: ctrl::compr_off(); break;
        default: err = ec::ArgInvalid; break;
    }
finish:
    msg.write(proto::TestCompr, err);
}

void HandlerTask::on_arm_adj(const message_t &msg) {
    robotArm1->adjust();
    robotArm2->adjust();
    msg.write(proto::ArmAdj, ec::Ok);
}

void HandlerTask::on_test_arm(const message_t &msg) {
    uint8_t type;
    uint16_t position;
    uint8_t speed;
    error_t err = msg.parse(type, position, speed);
    switch (type) {
        case 0x00: err = robotArm1->run(RobotArm1::ORIGIN_POSITION, speed); break;
        case 0x01: err = robotArm1->run(position, speed); break;
        case 0x02: robotArm1->set_ctrl_min(speed); break;
        case 0x03: err = robotArm2->run(RobotArm2::ORIGIN_POSITION, speed); break;
        case 0x04: err = robotArm2->run(position, speed); break;
        case 0x05: robotArm2->set_ctrl_min(speed); break;
        default: err = ec::ArgInvalid; break;
    }
finish:
    msg.write(proto::TestArm, err);
}

void HandlerTask::on_arm_ctrl(const message_t &msg) {
    uint16_t p1, p2;
    error_t err = msg.parse(p1, p2);
    if (err) {
        goto finish;
    }
    err = robot_arm::run(p1, p2);
finish:
    msg.write(proto::ArmCtrl, err);
}

void HandlerTask::on_auto_set_cargo(const message_t &msg) {
    cargo_t cargo;

    error_t err = msg.parse(cargo.row, cargo.col, cargo.depth1, cargo.depth2);
    if (err) {
        goto finish;
    }

    cargo.p1 = robotArm1->get_position();
    cargo.p2 = robotArm2->get_position();

    err = deviceInfo->change_or_add_cargo(cargo);

finish:
    msg.write(proto::AutoSetCargo, err, cargo.p1, cargo.p2);
}

void HandlerTask::on_set_cargo(const message_t &msg) {
    cargo_t cargo;
    error_t err = msg.parse(
        cargo.row, 
        cargo.col, 
        cargo.p1,
        cargo.p2,
        cargo.depth1,
        cargo.depth2
    );
    if (err) {
        goto finish;
    }
    err = deviceInfo->change_or_add_cargo(cargo);
finish:
    msg.write(proto::SetCargo, err);
}

void HandlerTask::on_get_cargo(const message_t &msg) {
    cargo_t cargo;
    error_t err = msg.parse(cargo.row, cargo.col);
    if (err) {
        goto finish;
    }
    err = deviceInfo->get_by_cargo(cargo);
finish:
    msg.write(proto::QueryCargo, 
        err,
        cargo.p1,
        cargo.p2,
        cargo.depth1,
        cargo.depth2
    );
}

void HandlerTask::on_get_all_cargo(const message_t &msg) {
    deviceInfo->notify_info(msg);
    msg.write(proto::QueryAllCargo, ec::Ok);
}

void HandlerTask::on_del_cargo(const message_t &msg) {
    cargo_t cargo;
    error_t err = msg.parse(cargo.row, cargo.col);
    if (err) {
        goto finish;
    }
    err = deviceInfo->del_cargo(cargo);
finish:
    msg.write(proto::DelCargo, err);
}

void HandlerTask::on_test_cargo(const message_t &msg) {
    cargo_t cargo;
    error_t err = msg.parse(cargo.row, cargo.col);
    if (err) {
        goto finish;
    }
    err = delivery::cargo_test(cargo);
finish:
    msg.write(proto::TestCargo, err);
}

void HandlerTask::on_auto_set_pick(const message_t &msg) {
    uint16_t depth;
    error_t err = msg.parse(depth);
    if (err) {
        goto finish;
    }
    deviceInfo->set_pick_info(depth);
finish:
    msg.write(proto::AutoSetPick, err);
}

void HandlerTask::on_set_pick(const message_t &msg) {
    uint16_t p1, p2, depth;
    error_t err = msg.parse(p1, p2, depth);
    if (err) {
        goto finish;
    }
    deviceInfo->set_pick_info(p1, p2, depth);
finish:
    msg.write(proto::SetPick, err);
}

void HandlerTask::on_get_param(const message_t &msg) {
    uint16_t p1, p2, depth;
    uint8_t r1_min, r2_min;
    uint16_t tube_sp, fridge_sp;

    deviceInfo->get_pick_info(p1, p2, depth);
    r1_min = robotArm1->get_ctrl_min();
    r2_min = robotArm2->get_ctrl_min();
    tube_sp = tube->get_target_speed();
    fridge_sp = fridge->get_target_speed();

    msg.write(proto::GetParam, 
        p1, p2, depth,
        r1_min, r2_min,
        tube_sp, fridge_sp
    );
}

void HandlerTask::on_open_lock(const message_t &msg) {
    ctrl::elock_open();
    msg.write(proto::ELock, ec::Ok);
}

void HandlerTask::on_write_motor_driver(const message_t &msg) {
    uint8_t index;
    uint8_t addr;
    uint16_t value;
    error_t err = msg.parse(index, addr, value);
    if (err) {
        goto finish;
    }
    switch (index) {
        case 1: motorDrv->write1(addr, value); break;
        case 2: motorDrv->write2(addr, value); break;
        default: err = ec::ArgInvalid; break;
    }
finish:
    msg.write(proto::WriteMotorDriver, err);
}

void HandlerTask::on_read_motor_driver(const message_t &msg) {
    uint8_t index;
    uint8_t addr;
    uint16_t value;
    error_t err = msg.parse(index, addr);
    if (err) {
        goto finish;
    }
    switch (index) {
        case 1: value = motorDrv->read1(addr); break;
        case 2: value = motorDrv->read2(addr); break;
        default: err = ec::ArgInvalid; break;
    }
finish:
    msg.write(proto::ReadMotorDriver, err, value);
}

void HandlerTask::on_get_drv_status(const message_t &msg) {
    uint8_t s1 = motorDrv->read_status1();
    uint8_t s2 = motorDrv->read_status2();
    msg.write(proto::GetDrvStatus, s1, s2);
}

void HandlerTask::on_clear_drv_status(const message_t &msg) {
    uint8_t index;
    error_t err = msg.parse(index);
    if (err) {
        goto finish;
    }
    switch (index) {
        case 1: motorDrv->clear1(); break;
        case 2: motorDrv->clear2(); break;
        default: err = ec::ArgInvalid; break;
    }
finish:
    msg.write(proto::ClearDrvFault, err);
}

}
